// 初始化PTP头并生成链路延时报文以及发送链路延时报文
#include <stdio.h>
#include <unistd.h>
#include <iostream>
#include <cstring>
#include <netinet/in.h> // 与 Internet 协议族相关的数据结构
#include "ptp.h"
#include "../tsmp/tsmp.h"
#include <cmath>

const uint32_t MAX_PKT_LEN = 2048;
// uint64_t clock_identity = 0x00;
uint8_t buffer_[MAX_PKT_LEN];
uint8_t src_mac[6];
uint8_t dst_mac[6];
uint8_t clock_identify[8];

Port_delay_state link_delay_state[MAX_PORT_NUM];
extern Ptp_Sync ptp_sync;
// Port_delay port_delay; // 转时间的 ptp时间戳转为64位

node_delay_state node;

extern LinkdelayInfo delayinfo;
// 发送Pdelay_req
uint8_t *sendPdelayReq(uint16_t sequence_id, int port_id)
{
    if (link_delay_state[port_id] == Port_delay_state::NEIGHIBOR_YES || link_delay_state[port_id] == Port_delay_state::DONE)
    {
        node.port_delay[port_id].last_timestamp_1 = node.port_delay[port_id].timestamp_1;
        node.port_delay[port_id].last_timestamp_2 = node.port_delay[port_id].timestamp_2;
        node.port_delay[port_id].last_timestamp_3 = node.port_delay[port_id].timestamp_3;
        node.port_delay[port_id].last_timestamp_4 = node.port_delay[port_id].timestamp_4;
    }
    PtpHeader pdelay_req_header;
    init(&pdelay_req_header, E_PDELAY_REQ, sequence_id); // 初始化PTP头
    PtpPdelayReq pdelay_req;
    memcpy(&pdelay_req.ptp_header, &pdelay_req_header, sizeof(pdelay_req_header));
    memset(pdelay_req.padding, 0, sizeof(pdelay_req.padding));
    memcpy(buffer_, &pdelay_req, sizeof(pdelay_req));
    link_delay_state[port_id] = Port_delay_state::SEND_REQ;
    printf("发送req的序列号：%u\n", sequence_id);
    return buffer_;
}

// 发送Announce
uint8_t *sendAnnounce(uint16_t sequence_id, int port_id)
{
    PtpHeader announce_header;
    init(&announce_header, E_ANNOUNCE, sequence_id); // 初始化PTP头
    PtpAnnounce announce;
    memcpy(&announce.ptp_header, &announce_header, sizeof(announce_header));
    memset(announce.reserved, 0, sizeof(announce.reserved));
    announce.current_utc_offset = htons(0x25);
    announce.reserved1 = 0x00;
    announce.gm_priority1 = 126;
    announce.gm_clock_class = 0xf8;
    announce.gm_clock_accuracy = 0xfe;
    announce.gm_clock_var = htons(0xffff);
    announce.gm_priority2 = 0x80;
    announce.gm_identify[0] = 0xa0;
    announce.gm_identify[1] = 0x36;
    announce.gm_identify[2] = 0x9f;
    announce.gm_identify[3] = 0xff;
    announce.gm_identify[4] = 0xfe;
    announce.gm_identify[5] = 0xa7;
    announce.gm_identify[6] = 0xa4;
    announce.gm_identify[7] = 0x96;
    announce.steps_removed = 0x00;
    announce.time_source = 0xa0;
    memcpy(buffer_, &announce, sizeof(announce));
    printf("发送announce的序列号：%u\n", sequence_id);
    return buffer_;
}

// 发送Pdelay_resp
uint8_t *sendPdelayResp(int port_id)
{
    PtpHeader resp_header;
    init(&resp_header, E_PDELAY_RESP, node.port_delay[port_id].ex_sequence_id); // 初始化PTP头，赋值外部sequence_id
    PtpPdelayResp resp;
    memcpy(&resp.ptp_header, &resp_header, sizeof(resp_header));
    uint8_t t2[10] = {0};
    e2t(node.port_delay[port_id].timestamp_2_ex, t2); // 将八字节的timestamp2转化成十字节，填到报文中
    memcpy(&resp.request_receipt_timestamp, &t2, sizeof(t2));
    memcpy(&resp.clock_identify, &clock_identify, sizeof(resp_header.clock_identify));
    resp.port_number = htons(port_id);
    memcpy(buffer_, &resp, sizeof(resp));
    printf("发送resp的序列号：%u\n", node.port_delay[port_id].ex_sequence_id);
    return buffer_;
}

// 发送Pdelay_resp_follow_up
uint8_t *sendPdelayRespFollowUp(int port_id)
{
    PtpHeader resp_header;
    init(&resp_header, E_PDELAY_RESP_FOLLOW_UP, node.port_delay[port_id].ex_sequence_id); // 初始化PTP头，赋值外部sequence_id
    PtpPdelayRespFollowUp respfollowup;
    memcpy(&respfollowup.ptp_header, &resp_header, sizeof(resp_header));
    uint8_t t3[10] = {0};
    e2t(node.port_delay[port_id].timestamp_3_ex, t3); // 将八字节的timestamp3转化成十字节，填到报文中
    memcpy(&respfollowup.request_origin_timestamp, &t3, sizeof(t3));
    memcpy(&respfollowup.clock_identify, &clock_identify, sizeof(resp_header.clock_identify));
    respfollowup.port_number = htons(port_id);
    memcpy(buffer_, &respfollowup, sizeof(respfollowup));
    printf("发送resp follow up的序列号：%u\n", node.port_delay[port_id].ex_sequence_id);
    return buffer_;
}

// 接收PdelayReq帧
int parsePdelayReq(struct Pack_rec tsmp_pack, int port_id) // 需要tsmp
{
    struct PtpPdelayReq req;
    memcpy(&req, &tsmp_pack.payload, sizeof(PtpPdelayReq));
    node.port_delay[port_id].ex_sequence_id = ntohs(req.ptp_header.sequence_id); // 把序列号赋值
    printf("我收到的req序列号：%u\n", ntohs(req.ptp_header.sequence_id));
    memcpy(&clock_identify, &req.ptp_header.clock_identify, sizeof(clock_identify));
    // node.port_delay[port_id].port_number = req.ptp_header.port_number;
    return 0;
}

// 接收PdelayResp帧
int parsePdelayResp(struct Pack_rec tsmp_pack, int port_id) // 需要ptp  no 24
{
    if (link_delay_state[port_id] != Port_delay_state::SEND_REQ) // 怀疑此处判断状态机有问题，不确定是否在requester.cpp中调用了sendPdelayReq函数后状态机的改变是否保存了。
    {
        std::cout << "recv resp pkt,state_ = " << (int)link_delay_state[port_id];
        return -1;
    }

    struct PtpPdelayResp resp;
    memcpy(&resp, &tsmp_pack.payload, sizeof(PtpPdelayResp));
    resp.ptp_header.sequence_id = ntohs(resp.ptp_header.sequence_id); // 更改字节序
    printf("我收到的resp序列号：%u\n", resp.ptp_header.sequence_id);
    // node.port_delay[port_id].port_number = resp.ptp_header.port_number;
    if (resp.ptp_header.sequence_id != node.port_delay[port_id].local_sequence_id) // 序列号等于本地序列号
    {
        printf("sequence id error, resp sequence id is %x, local sequence id is %x\n", resp.ptp_header.sequence_id, node.port_delay[port_id].local_sequence_id);
        return -1;
    }
    // node.port_delay[port_id].sequence_id = resp.ptp_header.sequence_id; // 将报文中的序列号赋值给node.port_delay[port_id].sequence_id
    uint64_t t2 = t2e(resp.request_receipt_timestamp); // 将十字节的报文中的时间填写到timestamp2中
    node.port_delay[port_id].timestamp_2 = t2;
    link_delay_state[port_id] = Port_delay_state::RECV_RESP; // 填状态机

    return 0;
}

// 接收PdelayRespFollowUp帧
int parsePdelayRespFollowUp(struct Pack_rec tsmp_pack, int port_id)
{
    if (link_delay_state[port_id] != Port_delay_state::RECV_RESP)
    {
        std::cout << "recv resp followup pkt,state_ = " << (int)link_delay_state[port_id];
        return -1;
    }

    struct PtpPdelayRespFollowUp resp_follow_up;
    memcpy(&resp_follow_up, &tsmp_pack.payload, sizeof(struct PtpPdelayRespFollowUp));
    resp_follow_up.ptp_header.sequence_id = ntohs(resp_follow_up.ptp_header.sequence_id); // 更改字节序
    printf("我收到的resp follow up序列号：%u\n", resp_follow_up.ptp_header.sequence_id);
    // node.port_delay[port_id].port_number = resp_follow_up.ptp_header.port_number;
    if (resp_follow_up.ptp_header.sequence_id != node.port_delay[port_id].local_sequence_id)
    {
        printf("sequence id error, resp follow up sequence id is %x, local sequence id is %x\n", resp_follow_up.ptp_header.sequence_id, node.port_delay[port_id].local_sequence_id);
        link_delay_state[port_id] = Port_delay_state::NONE;
        return -1;
    }
    // node.port_delay[port_id].sequence_id = resp_follow_up.ptp_header.sequence_id;
    uint64_t t3 = t2e(resp_follow_up.request_origin_timestamp); // 将十字节的报文中的时间填写到timestamp3中
    node.port_delay[port_id].timestamp_3 = t3;
    link_delay_state[port_id] = Port_delay_state::RECV_RESP_FOLLOW_UP;
    // printf("t1:%lx\n", node.port_delay[port_id].timestamp_1);
    // printf("t2:%lx\n", node.port_delay[port_id].timestamp_2);
    // printf("t3:%lx\n", node.port_delay[port_id].timestamp_3);
    // printf("t4:%lx\n", node.port_delay[port_id].timestamp_4);

    calculateLinkDelay(port_id);
    if (link_delay_state[port_id] == Port_delay_state::NEIGHIBOR_YES)
    {
        delayinfo.last_t1 = node.port_delay[port_id].last_timestamp_1;
        delayinfo.last_t2 = node.port_delay[port_id].last_timestamp_2;
        delayinfo.last_t3 = node.port_delay[port_id].last_timestamp_3;
        delayinfo.last_t4 = node.port_delay[port_id].last_timestamp_4;
        delayinfo.NeighborRateRatio = node.port_delay[port_id].neighbour_rateratio;
    }
    else
    {
        delayinfo.last_t1 = 0;
        delayinfo.last_t2 = 0;
        delayinfo.last_t3 = 0;
        delayinfo.last_t4 = 0;
        delayinfo.NeighborRateRatio = 0;
    }
    delayinfo.sequenceid = node.port_delay[port_id].local_sequence_id;
    delayinfo.Linkdelay = node.port_delay[port_id].link_delay;
    delayinfo.PortID = port_id;
    delayinfo.t1 = node.port_delay[port_id].timestamp_1;
    delayinfo.t2 = node.port_delay[port_id].timestamp_2;
    delayinfo.t3 = node.port_delay[port_id].timestamp_3;
    delayinfo.t4 = node.port_delay[port_id].timestamp_4;
    const char *filename_delay = "report/gPTPstate_linkdelay.txt";
    write_to_delayfile(delayinfo, filename_delay);

    return 0;
}

// 计算链路延时
int calculateLinkDelay(int port_id)
{
    if (link_delay_state[port_id] != Port_delay_state::RECV_RESP_FOLLOW_UP)
    {
        return -1;
    }

    // 提取秒和纳秒部分
    uint32_t seconds_1 = extract_seconds(node.port_delay[port_id].timestamp_1);
    uint32_t nanoseconds_1 = extract_nanoseconds(node.port_delay[port_id].timestamp_1);

    uint32_t seconds_2 = extract_seconds(node.port_delay[port_id].timestamp_2);
    uint32_t nanoseconds_2 = extract_nanoseconds(node.port_delay[port_id].timestamp_2);

    uint32_t seconds_3 = extract_seconds(node.port_delay[port_id].timestamp_3);
    uint32_t nanoseconds_3 = extract_nanoseconds(node.port_delay[port_id].timestamp_3);

    uint32_t seconds_4 = extract_seconds(node.port_delay[port_id].timestamp_4);
    uint32_t nanoseconds_4 = extract_nanoseconds(node.port_delay[port_id].timestamp_4);
    // 分别计算秒差和纳秒差
    int64_t seconds_diff_1 = (int64_t)seconds_4 - seconds_1;
    int64_t nanoseconds_diff_1 = (int64_t)nanoseconds_4 - nanoseconds_1;
    int64_t seconds_diff_2 = (int64_t)seconds_3 - seconds_2;
    int64_t nanoseconds_diff_2 = (int64_t)nanoseconds_3 - nanoseconds_2;

    // 处理纳秒部分的借位或进位
    if (nanoseconds_diff_1 < 0)
    {
        seconds_diff_1--;
        nanoseconds_diff_1 += 1000000000;
    }
    if (nanoseconds_diff_2 < 0)
    {
        seconds_diff_2--;
        nanoseconds_diff_2 += 1000000000;
    }

    // 计算最终的结果
    int64_t final_seconds_diff = (seconds_diff_1 - seconds_diff_2) / 2;
    int64_t final_nanoseconds_diff = (nanoseconds_diff_1 - nanoseconds_diff_2) / 2;
    // printf("final_seconds_diff:%ld\n", final_seconds_diff);
    // printf("final_nanoseconds_diff:%ld\n", final_nanoseconds_diff);

    // 处理最终的纳秒部分的借位或进位
    if (final_nanoseconds_diff < 0)
    {
        final_seconds_diff--;
        final_nanoseconds_diff += 1000000000;
    }
    // else if (final_nanoseconds_diff >= 1000000000)
    // {
    //     final_seconds_diff++;
    //     final_nanoseconds_diff -= 1000000000;
    // }

    // 将结果合并回一个uint64_t变量
    uint64_t final_timestamp = ((uint64_t)final_seconds_diff << 32) | final_nanoseconds_diff;

    node.port_delay[port_id].link_delay = final_timestamp;
    if (node.port_delay[port_id].link_delay > 10000)
    {
        printf("-------------------------------------------------------------------------------------------------------------\n");
        printf("----------t1:%u\n", nanoseconds_1);
        printf("----------t2:%u\n", nanoseconds_2);
        printf("----------t3:%u\n", nanoseconds_3);
        printf("----------t4:%u\n", nanoseconds_4);
    }

    printf("\n\n node.port_delay[%d]->link_delay: %ld ns\n\n", port_id, node.port_delay[port_id].link_delay);
    // node.port_delay[port_id].link_delay = 0;
    link_delay_state[port_id] = Port_delay_state::DONE;
    calculateNR(port_id);

    return 0;
}

// 计算邻居频比
int calculateNR(int port_id)
{
    if (node.port_delay[port_id].last_timestamp_3 == 0 || node.port_delay[port_id].last_timestamp_4 == 0)
    {
        printf("just one link delay, I need two!!!\n");
        return 0;
    }
    // 提取秒和纳秒部分
    uint32_t seconds_3 = extract_seconds(node.port_delay[port_id].timestamp_3);
    uint32_t nanoseconds_3 = extract_nanoseconds(node.port_delay[port_id].timestamp_3);
    uint32_t last_seconds_3 = extract_seconds(node.port_delay[port_id].last_timestamp_3);
    uint32_t last_nanoseconds_3 = extract_nanoseconds(node.port_delay[port_id].last_timestamp_3);
    uint32_t seconds_4 = extract_seconds(node.port_delay[port_id].timestamp_4);
    uint32_t nanoseconds_4 = extract_nanoseconds(node.port_delay[port_id].timestamp_4);
    uint32_t last_seconds_4 = extract_seconds(node.port_delay[port_id].last_timestamp_4);
    uint32_t last_nanoseconds_4 = extract_nanoseconds(node.port_delay[port_id].last_timestamp_4);

    // 计算秒差和纳秒差
    int64_t seconds_diff_3 = (int64_t)seconds_3 - last_seconds_3;
    int64_t nanoseconds_diff_3 = (int64_t)nanoseconds_3 - last_nanoseconds_3;
    int64_t seconds_diff_4 = (int64_t)seconds_4 - last_seconds_4;
    int64_t nanoseconds_diff_4 = (int64_t)nanoseconds_4 - last_nanoseconds_4;

    // 处理纳秒部分的借位或进位
    if (nanoseconds_diff_3 < 0)
    {
        seconds_diff_3--;
        nanoseconds_diff_3 += 1000000000;
    }
    if (nanoseconds_diff_4 < 0)
    {
        seconds_diff_4--;
        nanoseconds_diff_4 += 1000000000;
    }

    // 计算除法分母部分
    double divisor = 1e9 * seconds_diff_3 + nanoseconds_diff_3;
    // printf("divisor:%f\n", divisor);
    if (divisor == 0.0)
    {
        // 避免除以零的错误
        printf("Error: Division by zero.\n");
        return 1;
    }
    double fenzi = 1e9 * seconds_diff_4 + nanoseconds_diff_4;
    // printf("fenzi:%f\n", fenzi);
    double result_double = (1e9 * seconds_diff_4 + nanoseconds_diff_4) / divisor;

    node.port_delay[port_id].neighbour_rateratio = result_double;
    printf("node.port_delay[%d].neighbour_rateratio:%.10f\n", port_id, node.port_delay[port_id].neighbour_rateratio);
    link_delay_state[port_id] = Port_delay_state::NEIGHIBOR_YES;

    return 0;
}

// 将本节点的GM频率比写进寄存器
int ticklen_write(int port_id)
{
    uint32_t addr = 0x00080002;
    if (link_delay_state[port_id] != Port_delay_state::NEIGHIBOR_YES)
    {
        return 0;
    }
    // 获取csro,上一节点的GM频率比
    int32_t cumulativeScaledRateOffset = 0; // ptp_sync.cumulativeScaledRateOffset; // 测试
    // 获取邻居频比
    double nr = node.port_delay[port_id].neighbour_rateratio;
    int std_f = 125 * 1e6; // 千兆标称125Mhz晶振
    double ticklen = (1 / std_f) * ((double)cumulativeScaledRateOffset * pow(2, -41) + nr);
    // std::vectortor<uint32_t> vector; // 将64位ticklen转化为两个32位数组
    ptp_sync.cumulativeScaledRateOffset = cumulativeScaledRateOffset + (nr - 1) * pow(2, 41);
    uint32_t vector[2];             // 寄存器高低位，总共8个字节，写两个值
    uint32_t high_part, low_part;   // 高低位
    uint32_t a = (uint32_t)ticklen; // 获取整数部分
    a = a << 9;                     // a向左移9位
    // 取得ticklen的整数部分
    double ticklen_integer_part = trunc(ticklen);
    // 取得ticklen的小数部分
    double ticklen_decimal_part = ticklen - ticklen_integer_part;
    uint64_t b = (uint64_t)(ticklen_decimal_part * pow(2, 41)); // 小数部分乘以2的41次方
    low_part = (uint32_t)b;                                     // 获取低32位
    b = b >> 32;                                                // 再往左移32位
    high_part = a | b;                                          // 将前32位进行操作
    printf("---------------highpart:%d\n", high_part);
    printf("---------------lowpart:%d\n", low_part);
    // vector.push_back(high_part);
    // vector.push_back(low_part);
    vector[0] = high_part;
    vector[1] = low_part;
    int iRet;
    // 写入寄存器
    iRet = writeReqSend(addr, 2, vector);
    if (iRet == -1)
    {
        std::cout << "write data error" << std::endl;
        return -1;
    }
    iRet = readReqSend(addr, 2);
    if (iRet == -1)
    {
        std::cout << "read data error" << std::endl;
        return -1;
    }

    return 0;
}